import rclpy
from rclpy.node import Node
from sensor_msgs.msg import PointCloud2, PointField
import sensor_msgs_py.point_cloud2 as pc2

class LivoxPointCloudSubscriber(Node):
    def __init__(self):
        super().__init__('livox_pointcloud_subscriber')
        # 订阅 LivoxPointXyzrtlt 格式的点云话题
        self.subscription = self.create_subscription(
            PointCloud2,
            '/lidar_point_cloud',  # Livox 格式点云话题
            self.lidar_callback,
            10)
        self.subscription  # 防止回收变量

    def lidar_callback(self, msg):
        # 打印点云基本信息
        self.get_logger().info(f'Received point cloud with {msg.width} points')
        self.get_logger().info(f'Frame ID: {msg.header.frame_id}')
        self.get_logger().info(f'Stamp: {msg.header.stamp}')

        # 打印点云字段信息
        field_names = [field.name for field in msg.fields]
        self.get_logger().info(f'Point cloud fields: {field_names}')

        # 读取 LivoxPointXyzrtlt 格式点云数据
        points_list = pc2.read_points(
            msg, skip_nans=True, field_names=("x", "y", "z", "reflectivity", "tag", "line", "timestamp")
        )

        # 打印每个点的详细信息 (x, y, z, reflectivity, tag, line, timestamp)
        for point in points_list:
            x, y, z, reflectivity, tag, line, timestamp = point
            self.get_logger().info(
                f"Point: x={x}, y={y}, z={z}, reflectivity={reflectivity}, tag={tag}, "
                f"line={line}, timestamp={timestamp}"
            )

        self.get_logger().info("---- End of point cloud message ----")


def main(args=None):
    rclpy.init(args=args)

    livox_pointcloud_subscriber = LivoxPointCloudSubscriber()

    try:
        rclpy.spin(livox_pointcloud_subscriber)
    except KeyboardInterrupt:
        pass

    livox_pointcloud_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
